//Pietras93OS
//Scheduler
//pietras93@gmail.com
//

#include <system.h>

static unsigned current;
static unsigned char stacks[MAX_TASK][USER_STACK_SIZE]; 

char * UIntToString(unsigned int Val, char * Str, char Base)	//TODO:
{																//Put in lib!
char Result, strTmp[8], Pos, i;
unsigned int Value=Val;
Pos=0;
do
{
Result = Value % Base;
Value = Value / Base;
strTmp[Pos++] = Result>9?'A'+(Result-10):'0'+Result;
}while ( Value );
strTmp[Pos--]=0;
for(i=0;Pos>=0;Pos--,i++)
Str[i]=strTmp[Pos];
Str[i]=0;
return Str;
}

void schedule(void)
{
        if(setjmp(_curr_task->state) != 0)
          return;

        do
        {
            current++;
            if(current >= MAX_TASK)
              current = 0;
            _curr_task = _tasks + current;
        }
        while(_curr_task->status != TS_RUNNABLE);
        longjmp(_curr_task->state, 1);
}



void init_tasks(void)
{
        int i;
		char *strih, bs = ' ';
        //printf("\nInitialising Tasks\t: ");
        // Here we should malloc the stacks
        int j;
        for(i = 1; i < (NUM_TASKS+1); i++) // Create a few demo tasks
          j = add_task((unsigned)demotask, "root", PR_NORMAL, "demotask");
        _tasks[0].status = TS_RUNNABLE; // Idle task 0
        _curr_task = _tasks + 0;
        _tasks[0].owner = "system";
        _tasks[0].priority = PR_LOW;
        _tasks[0].desc = "System Idle";
        _tasks[0].pid = 0;

        int volatile tsk = 0;
        for ( i = 0; i < MAX_TASK; i++)
        {
              if (_tasks[i].status == TS_RUNNABLE)
              {
                      tsk++;
              }
        }
		UIntToString(tsk, strih, bs);
        puts("Done. ");
		puts(strih);
		puts(" active task(s).\n");
}

int gettask()
{
        return _curr_task->pid;
}

int add_task(unsigned taskaddress, char* owner, int priority, char *desc)
{
        int volatile tr = 0;
        unsigned adr;
        do
        {
              tr++;
              if(tr >= MAX_TASK)
                 tr = 0;
        }
        while(_tasks[tr].status != TS_FREE);

              (void)setjmp(_tasks[tr].state);
              adr = (unsigned)(stacks[tr] + USER_STACK_SIZE);
              _tasks[tr].state[0].JMPBUF_SP = adr;
              _tasks[tr].state[0].JMPBUF_IP = (unsigned)taskaddress;
              _tasks[tr].state[0].JMPBUF_FLAGS = 0x200;
              _tasks[tr].window = 0;
              _tasks[tr].status = TS_ZOMBIE;
              _tasks[tr].owner = owner;
              _tasks[tr].priority = priority;
              _tasks[tr].desc = desc;
              _tasks[tr].pid = tr;

              return tr;
}

void kill_task(int pid)
{
        int tr = NULL;
        do
        {
              tr++;
              if(tr >= MAX_TASK)
                 tr = 0;
        }
        while(_tasks[tr].pid != pid);
        _tasks[tr].status = TS_FREE;
		void *p = &_tasks[tr].state[0];
        free(p);
}

#define WAIT 0xFFFFFL

extern task_t *_curr_task;

void yield(void);
void wait(void);
void schedule(void);

int pp;

void yield(void)
{
      schedule();
}

void wait(void)
{
      unsigned long wait;
      for(wait = WAIT; wait != 0; wait--)
      /* nothing */;
}

void demotask(void)
{
	  int i;
	  char c;
      wait();
      for(i=1; i==10; i++)
      {
           yield();
           int t = gettask();
		   itoa(i, c);
           puts(c);
		   puts("\t");
           wait();
      }
}
